﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Woodstock.Common.Filters
{
    public class KalmanFilter
    {     
        struct Gyro1DKalman
        {
	        /* These variables represent our state matrix x */
	        public double x_angle,
	              x_bias;

	        /* Our error covariance matrix */
            public double P_00,
                P_01,
                P_10,
                P_11;	
	
	        /* 
	         * Q is a 2x2 matrix of the covariance. Because we
	         * assuma the gyro and accelero noise to be independend
	         * of eachother, the covariances on the / diagonal are 0.
	         *
	         * Covariance Q, the process noise, from the assumption
	         *    x = F x + B u + w
	         * with w having a normal distribution with covariance Q.
	         * (covariance = E[ (X - E[X])*(X - E[X])' ]
	         * We assume is linair with dt
	         */
            public double Q_angle, Q_gyro;
	        
            /*
	         * Covariance R, our observation noise (from the accelerometer)
	         * Also assumed to be linair with dt
	         */
            public double R_angle;
        };

        Gyro1DKalman internalData = new Gyro1DKalman();

        void init(SpatialState spatialData, double Q_angle, double Q_gyro, double R_angle)
        {
	        internalData.Q_angle = Q_angle;
	        internalData.Q_gyro  = Q_gyro;
	        internalData.R_angle = R_angle;
        }




        /*
         * The predict function. Updates 2 variables:
         * our model-state x and the 2x2 matrix P
         *     
         * x = [ angle, bias ]' 
         * 
         *   = F x + B u
         *
         *   = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
         *
         *   => angle = angle + dt (dotAngle - bias)
         *      bias  = bias
         *
         *
         * P = F P transpose(F) + Q
         *
         *   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
         *
         *  P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt² * P(1,1) + Q(0,0)
         *  P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
         *  P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
         *  P(1,1) = P(1,1) + Q(1,1)
         *
         *
         */
        void ars_predict(ref SpatialState spatialData, double dotAngle, double dt)
        {
	        internalData.x_angle += dt * (dotAngle - internalData.x_bias);

	        internalData.P_00 +=  - dt * (internalData.P_10 + internalData.P_01) + internalData.Q_angle * dt;
	        internalData.P_01 +=  - dt * internalData.P_11;
	        internalData.P_10 +=  - dt * internalData.P_11;
	        internalData.P_11 +=  + internalData.Q_gyro * dt;
        }

        /*
         *  The update function updates our model using 
         *  the information from a 2nd measurement.
         *  Input angle_m is the angle measured by the accelerometer.
         *
         *  y = z - H x
         *
         *  S = H P transpose(H) + R
         *    = [ 1 0 ] P [ 1, 0 ] + R
         *    = P(0,0) + R
         * 
         *  K = P transpose(H) S^-1
         *    = [ P(0,0), P(1,0) ] / S
         *
         *  x = x + K y
         *
         *  P = (I - K H) P
         *
         *    = ( [ 1 0,    [ K(0),
         *          0 1 ] -   K(1) ] * [ 1 0 ] ) P
         *
         *    = [ P(0,0)-P(0,0)*K(0)  P(0,1)-P(0,1)*K(0),
         *        P(1,0)-P(0,0)*K(1)  P(1,1)-P(0,1)*K(1) ]
         */
        double ars_update(ref SpatialState spatialData, double angle_m)
        {
            double y = angle_m - internalData.x_angle;

            double S = internalData.P_00 + internalData.R_angle;
            double K_0 = internalData.P_00 / S;
            double K_1 = internalData.P_10 / S;
	
	        internalData.x_angle +=  K_0 * y;
	        internalData.x_bias  +=  K_1 * y;
	
	        internalData.P_00 -= K_0 * internalData.P_00;
	        internalData.P_01 -= K_0 * internalData.P_01;
	        internalData.P_10 -= K_1 * internalData.P_00;
	        internalData.P_11 -= K_1 * internalData.P_01;
	
	        return internalData.x_angle;
        }

    }
}
